// SPDX-License-Identifier: GPL-2.0-or-later
/*
 * A generic kernel FIFO implementation
 */
#include <string.h>
#include "kfifo.h"


void kfifo_init(KFIFO *fifo, void *buffer, uint32_t size)
{
    /* size 必须是2的n次方 */
    fifo->size = size - 1;
    fifo->in = fifo->out = 0;
    fifo->data = buffer;
}

/* 入队 */
uint32_t kfifo_in(KFIFO *fifo, const void *src, int32_t len)
{
    uint32_t len_tail;

    if((fifo->data == NULL) || (kfifo_avail(fifo) < len))
        return 0;

    /* 计算 data 尾部剩余长度 */
    len_tail = kfifo_size(fifo) - (fifo->in & fifo->size);
    len_tail = len_tail < len ? len_tail : len;

    memcpy(fifo->data + (fifo->in & fifo->size), src, len_tail);
    memcpy(fifo->data, (uint8_t *)src + len_tail, len - len_tail);

    fifo->in += len;

    return len;
}

/* 出队 */
uint32_t kfifo_out(KFIFO *fifo, void *dst, int32_t len)
{
    uint32_t len_tail;

    if((fifo->data == NULL) || (kfifo_len(fifo) < len))
        return 0;

    /* 计算 data 尾部已存储长度 */
    len_tail = kfifo_size(fifo) - (fifo->out & fifo->size);
    len_tail = len_tail < len ? len_tail : len;

    memcpy(dst, fifo->data + (fifo->out & fifo->size), len_tail);
    memcpy((uint8_t *)dst + len_tail, fifo->data, len - len_tail);

    fifo->out += len;

    return len;
}

/* 获取队列中到下一个分隔符的数据长度 */
uint32_t kfifo_len_need(KFIFO *fifo, uint8_t byte)
{
    uint8_t *pbyte;
    uint32_t len = kfifo_len(fifo), len_tail;

    /* 计算 data 尾部已存储长度 */
    len_tail = kfifo_size(fifo) - (fifo->out & fifo->size);
    len_tail = len_tail < len ? len_tail : len;

    if((pbyte = memchr(fifo->data + (fifo->out & fifo->size), byte, len_tail)) != NULL)
        return pbyte - (fifo->data + (fifo->out & fifo->size)) + 1;

    else if((pbyte = memchr(fifo->data, byte, len - len_tail)) != NULL)
        return pbyte - fifo->data + 1 + len_tail;

    return 0;
}

uint32_t kfifo_put(KFIFO *fifo, uint8_t byte)
{
    return kfifo_in(fifo, &byte, 1);
}

uint32_t kfifo_get(KFIFO *fifo, uint8_t *byte)
{
    return kfifo_out(fifo, byte, 1);
}
